#include "bits/stdc++.h"
#include "path.h"
#include "unistd.h"

using namespace std;

#include "ros/ros.h"
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
#include <sensor_msgs/LaserScan.h>

auto pt        = new path();
auto msg_copy  = new my_msg::LaserScans();
int  generated = 0;

void laserCallback(const sensor_msgs::LaserScan::ConstPtr& msg)
{
    msg_copy->angle_min       = msg->angle_min;
    msg_copy->angle_increment = msg->angle_increment;
    size_t t                  = msg->ranges.size();
    for ( size_t i = 0; i < t; i++ ) {
        msg_copy->ranges[i] = msg->ranges[i];
    }
    cout << "jinle" << endl;

    if ( generated == 0 ) {
        if ( pt->generate(msg_copy) == 0 ) {
            if ( pt->type == HEAD ) {
                ros::param::set("speed", 3);
                ros::param::set("gear", 10);
            }
            else if ( pt->type == BACK ) {
                ros::param::set("speed", -3);
                ros::param::set("gear", 0);
            }
        }
        else {
            generated = 1;
        }
    }
    else if ( pt->flag == 2 ) {
        ros::param::set("gear", 0);
        ros::param::set("speed", 0);
        ros::param::set("state", END);
        cout << "done!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" << endl;
    }
    else {
        int gear_t = pt->count_path(msg_copy);
        ros::param::set("gear", gear_t);
        if ( pt->type == HEAD ) {
            ros::param::set("speed", 3);
        }
        else if ( pt->type == BACK ) {
            ros::param::set("speed", -3);
        }
    }
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "path");
    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe("/scan", 1, laserCallback);
    ros::Rate       looprate(50);

    int state = 0;
    int type  = 0;
    pt->type  = BACK;
    while ( ros::ok() ) {
        ros::param::get("state", state);
        if ( state == DAOCHE || state == CEFANG ) {
            ros::spinOnce();
        }
        looprate.sleep();
    }
    return 0;
}
